#include "podserver.h"
#include <cstring>
#include <cstdio>

#define POLL_INTERVAL 0.025 // 40 per second

Pod PodServer::pod;
SocketServer PodServer::server(TCP_LISTEN_PORT, MAX_NUM_CONNECTIONS);
bool PodServer::polling = false;
bool PodServer::moving = false;
bool PodServer::verbose = true;
bool PodServer::verbose_poll = true;
bool PodServer::verbose_move = false;
Socket* PodServer::poll_client = NULL;
Socket* PodServer::move_client = NULL;
int PodServer::poll_count = 0;
int PodServer::move_count = 0;

/*
 * Constructor
 */
PodServer::PodServer(void) {}

/*
 * Main server loop
 */
void PodServer::run(void)
{
   bool exit = false;
   podcommand type;
   pod.start();
   while(!exit) {
      // read sockets and serve client commands
      if(!server.readSockets()) {
          continue;
      }
      for(unsigned int i = 0; i < server.clients.size(); ++i) {
         Socket* client = server.clients[i];
         if(server.readable(client) && client!=move_client && client!=poll_client) {
             int r = server.receiveUnsignedInt(client,(unsigned int*)&type);
             if(r <= 0) {
                 server.receiveError(r, client);
                 break;
             }
             process_command(type, client);
         }
      }
   }
}

/*
 * Given a protocol command and a client, serve the request
 */
void PodServer::process_command(podcommand type, Socket *client)
{
   int r;
   switch(type)
   {
      case cmdMOVE:
         movedata mdata;
         r = server.receiveStruct(client, &mdata,sizeof(movedata));
         if(r <= 0) {
            server.receiveError(r, client);
            moving = false;
            break;
         }
         if(r != sizeof(movedata)) {
            printf("Received incomplete move data packet\n");
         }
         pod.x = mdata.x;
         pod.y = mdata.y;
         pod.z = mdata.z;
         pod.roll = mdata.r;
         pod.pitch = mdata.p;
         pod.yaw = mdata.w;
         if(verbose) {
            fprintf(stdout, "cmdMOVE: %f %f %f %f %f %f\n", 
               mdata.x, mdata.y, mdata.z, mdata.r, mdata.p, mdata.w);
         }
         break;

      case cmdPOLLINPUTS:
         if(verbose) {
            for(int i = 0; i < NUM_INPUTS; ++i){
               printf("%i\t",pod.inputs[i]);
            }
            printf("\n");
         }
         r = server.sendStruct(client, &pod.inputs, sizeof(polldata));
         if(r == -1) {
            server.sendError(r, client);
            polling = false;
            break;
         }
         if(r != sizeof(polldata)) {
            printf("Sent incomplete poll data packet\n");
         }
         break;

      case cmdSTARTPOLLING:
         if(!polling) {
            poll_client = client;
            if(verbose) printf("Sending poll data to client %i\n", poll_client->id);
            polling = true;
            _beginthreadex(NULL,0, &poll_loop, NULL, 0, NULL);
         } else {
            printf("Polling thread is already running\n");
         }
         break;
      case cmdSTOPPOLLING:
         if(polling) {
            polling=false;
         } else {
            printf("Polling thread was not running\n");
         }
         break;

      case cmdSTARTMOVING:
         if(!moving) {
            move_client = client;
            if(verbose) printf("Receiving move commands from client %i\n", move_client->id);
            moving = true;
            _beginthreadex(NULL,0, &move_loop, NULL, 0, NULL);
         } else {
            printf("Move thread is already running\n");
         }
         break;
      case cmdSTOPMOVING:
         if(moving) {
            moving=false;
         } else {
            printf("Move thread was not running\n");
         }
         break;

      case cmdSTART:
         if(verbose) printf("Pod started\n");
         pod.start();
         break;
      case cmdSTOP:
         if(verbose) printf("Pod stopped\n");
         pod.stop();
         break;
      case cmdHOME:
         if(verbose) printf("Pod home\n");
         pod.home();
         break;
      case cmdPARK:
         if(verbose) printf("Pod parked\n");
         pod.park();
         break;
      case cmdFOLLOW:
         if(verbose) printf("Pod following\n");
         pod.follow();
         break;
      case cmdGETSTATE:
         const char* state;
         state = pod.get_state();
         if(verbose) printf("State: %s\n",state);
         server.sendString(client, (char *)state);
         break;
      case cmdVERBOSE:
         verbose = !verbose;
         break;
      case cmdVERBOSEPOLL:
         verbose_poll = !verbose_poll;
         if(!verbose_poll) printf("\n");
         break;
      case cmdVERBOSEMOVE:
         verbose_move = !verbose_move;
         if(!verbose_move) printf("\n");
         break;
      case cmdERROR:
         if(verbose) fprintf(stderr, "Error!\n");
         break;
      case cmdNOOP:
         break;
   }
}

/*
 * At each time interval, sends a structure containing all input values
 *
 * INPUTS:
 * Analog   0: Joystick: Forwards(+) / Backwards(-)
 * Analog   1: Joystick: Right(+) / Left(-)
 * Analog   2: Throttle
 * Digital 20: Throttle Button
 * Digital 22: Joystick Thumb Button
 * Digital 23: Joystick Fire Button
 */
unsigned int __stdcall PodServer::poll_loop(void* a)
{
   static clock_t start = clock();
   double interval = POLL_INTERVAL * (double)CLOCKS_PER_SEC;
   clock_t goal;
   int r, i=0;
   if(verbose) printf("Started polling thread\n");
   while(polling)
   {
      // send the data
      r = server.sendStruct(poll_client, &pod.inputs, sizeof(polldata));
      if(r == -1) {
         server.sendError(r, poll_client);
         polling = false;
         continue;
      }
      if(r != sizeof(polldata)) {
         printf("Sent incomplete poll data packet\n");
      }
      poll_count++;

      // print out values if verbose
      if(verbose_poll) {
         printf("\r[%6i] x: %5i y: %5i t: %5i jt: %5i jf: %5i",
             poll_count, pod.inputs[0],pod.inputs[1],pod.inputs[2],
             pod.inputs[14],pod.inputs[15]);
      }

      //sleep until end of real time interval
      goal = start + (clock_t)interval;
      while(clock() < goal) Sleep(1);
      start = clock();
   }
   poll_client = NULL;
   poll_count = 0;
   if(verbose) printf("Stopped polling thread\n");
   _endthreadex(NULL);
   return 0;
}

/*
 * At each time interval, try and receive a move command
 */
unsigned int __stdcall PodServer::move_loop(void* a)
{
    movedata mdata;
    int r;
    if(verbose) printf("Started moving thread\n");
    while(moving)
    {
        // receive the data and update values in pod, will block until
        // it receives so no need to wait
        r = server.receiveStruct(move_client, &mdata, sizeof(movedata));
        if(r <=0) {
            server.receiveError(r, move_client);
            moving = false;
            continue;
        }
        if(r != sizeof(movedata)) {
            printf("Received incomplete move data packet\n");
        }
        pod.x = mdata.x;
        pod.y = mdata.y;
        pod.z = mdata.z;
        pod.roll = mdata.r;
        pod.pitch = mdata.p;
        pod.yaw = mdata.w;
        move_count++;

        // print out the received data if verbose
        if(verbose_move) {
           printf("\r[%6i] x: %6f y: %6f z: %6f r: %6f p: %6f w: %6f", 
                move_count, mdata.x, mdata.y, mdata.z, mdata.r, mdata.p, mdata.w);
        }
    }
    move_client = NULL;
    move_count = 0;
    if(verbose) printf("Stopped moving thread\n");
    _endthreadex(NULL);
    return 0;
}

int main(int argc, char* argv[]) {
   PodServer server;
   server.run();
   return 0;
}
